#include "CenterOfMassSensorVisualisation.h"

#include <Inventor/nodes/SoSphere.h>
#include <Inventor/nodes/SoCylinder.h>
#include <Inventor/nodes/SoMaterial.h>
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>

#include <tuple>

using namespace MMM;

CenterOfMassSensorVisualisation::CenterOfMassSensorVisualisation(WholeBodyDynamicSensorPtr sensor, SoSeparator* sceneSep) :
    SensorVisualisation(sceneSep),
    sensor(sensor)
{
}

std::string CenterOfMassSensorVisualisation::getType() {
    return sensor->getType();
}

int CenterOfMassSensorVisualisation::getPriority() {
    return sensor->getPriority();
}

Eigen::Matrix4f CenterOfMassSensorVisualisation::createCOMPose(const Eigen::Vector3f &centerOfMass) {
    Eigen::Vector3f com = centerOfMass;
    Eigen::Matrix4f globalPose = Eigen::Matrix4f::Identity();
    //com(2) = 0.0f;
    globalPose.block(0, 3, 3, 1) = com;
    return globalPose;
}

void CenterOfMassSensorVisualisation::setPose(const Eigen::Vector3f &pose) {
    SoMatrixTransform* mt = (SoMatrixTransform*) getChildVisualisationSep(1);
    SbMatrix m_(reinterpret_cast<SbMat*>(CenterOfMassSensorVisualisation::createCOMPose(pose).data()));
    mt->matrix.setValue(m_);
}

void CenterOfMassSensorVisualisation::update(float timestep, float delta) {
    update(sensor->getDerivedMeasurement(timestep, delta));
}

void CenterOfMassSensorVisualisation::update(float timestep) {
    update(sensor->getDerivedMeasurement(timestep));
}

void CenterOfMassSensorVisualisation::update(WholeBodyDynamicSensorMeasurementPtr measurement) {
    if (!measurement) return;

    if (!hasVisualisationSep()) {
        SoSeparator* comSep = new SoSeparator;
        SoMaterial* marked = new SoMaterial;
        marked->ambientColor.setValue(1.0f, 0.2f, 0.2f);
        marked->diffuseColor.setValue(1.0f, 0.2f, 0.2f);
        marked->specularColor.setValue(0.5f, 0.5f, 0.5f);
        comSep->addChild(marked);
        Eigen::Matrix4f centerOfMass = CenterOfMassSensorVisualisation::createCOMPose(measurement->getCenterOfMass());
        comSep->addChild(VirtualRobot::CoinVisualizationFactory::getMatrixTransform(centerOfMass));
        SoSphere* sphere = new SoSphere();
        sphere->radius = 50;
        comSep->addChild(sphere);
        setVisualisationSep(comSep);
    } else {
        setPose(measurement->getCenterOfMass());
    }
}
